#pragma once
#include "ros/ros.h"
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning.h"
#include "auto_msgs/auto_ctr.h"
#include "auto_msgs/planning_point.h"
#include "common_tool/clampPid.h"
#include "common_tool/watchDog.h"
#include <osqp_control/debug_lateral.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>
#include "yaml-cpp/yaml.h"

namespace auto_ros
{
namespace control
{
class NodeBase
{
	//    watchDog(int foodPerTurn=0, int hungerPerTurn=0, int hungerMin=0,\
    //int criticalPoint=50, int hungerMax=100);
protected:
	watchDog localizition_dog_;
	watchDog chassis_dog_;
	watchDog planning_dog_;
	watchDog auto_msg_dog_;
	auto_msgs::planning planning_msg_;
	auto_msgs::localizition localizition_msg_;
	auto_msgs::chassis chassis_msg_;
	auto_msgs::auto_ctr auto_ctr_msg_;
	osqp_control::debug_lateral debug_msg_;
	void NoFood();

	//ref point related
	auto_msgs::planning_point ref_point_closet_;
	auto_msgs::planning_point ref_point_second_closet_;
	auto_msgs::planning_point ref_point_interp_;
	double ref_lambda_ = 0;
	double lateral_error_ = 0;

	YAML::Node node_yaml_;
	std::string log_file_name_;

public:
	NodeBase() = delete;
	NodeBase(double loop_rate, std::string node_yaml_file_name);
	void CBLocalizition(const auto_msgs::localizition &msg);
	void CBChassis(const auto_msgs::chassis &msg);
	void CBPlanning(const auto_msgs::planning &msg);
	void CBAuto(const auto_msgs::auto_ctr &msg);
	bool SearchRefPoint(const auto_msgs::planning msg);
	bool CheckDog();

	virtual void LoopProc() = 0;
	virtual void HardBrake() = 0;
	virtual bool UpdateInput() = 0;
	virtual void UpdateDebugMsg() = 0;

	//relate with  ros
	// publish apollo wrapper topic
	ros::NodeHandle nh;
	ros::Publisher control_cmd_pub_ = nh.advertise<auto_msgs::control_cmd>("control_cmd", 3);
	auto_msgs::control_cmd control_cmd_;
	ros::Publisher debug_pub_ = nh.advertise<osqp_control::debug_lateral>("debug_lateral", 3);

	//pub debug msg
	//ros::Publisher pure_pursuit_debug_pub =nh.advertise
	//    <pure_pursuit_pkg::pure_pursuit_debug>("pure_pursuit_debug", 3);
	//subscribe apollo wrapper topic
	ros::Rate loop_rate_;
	double rate_;
	ros::Subscriber localizition_sub_ =
		nh.subscribe("localizition", 3, &NodeBase::CBLocalizition, this);
	ros::Subscriber chassis_sub_ =
		nh.subscribe("chassis", 3, &NodeBase::CBChassis, this);
	ros::Subscriber planning_sub_ =
		nh.subscribe("planning_path", 3, &NodeBase::CBPlanning, this);
	ros::Subscriber auto_sub_ =
		nh.subscribe("auto_msg", 3, &NodeBase::CBAuto, this);
};
} // namespace control
} // namespace auto_ros